import cv2
import mediapipe as mp
import time

import requests


class FaceDetector():
    def __init__(self, minDetectionCon=0.5):
        self.minDetectionCon = minDetectionCon
        self.mpFaceDetection = mp.solutions.face_detection
        self.mpDraw = mp.solutions.drawing_utils
        self.faceDetection = self.mpFaceDetection.FaceDetection(self.minDetectionCon)

    def findFaces(self, img, draw=True):
        imgRGB = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
        self.results = self.faceDetection.process(imgRGB)
        # print(self.results)
        bboxs = []
        if self.results.detections:
            for id, detection in enumerate(self.results.detections):
                bboxC = detection.location_data.relative_bounding_box
                ih, iw, ic = img.shape
                bbox = int(bboxC.xmin * iw), int(bboxC.ymin * ih), int(bboxC.width * iw), int(bboxC.height * ih)
                bboxs.append([id, bbox, detection.score])
                if draw:
                    img = self.fancyDraw(img, bbox)
                    cv2.putText(img, f'{int(detection.score[0] * 100)}%', (bbox[0], bbox[1] - 20),
                                cv2.FONT_HERSHEY_PLAIN, 2, (255, 0, 255), 2)
                return img, bboxs

    def fancyDraw(self, img, bbox, l=30, t=5, rt=1):
        x, y, w, h = bbox
        x1, y1 = x + w, y + h
        cv2.rectangle(img, bbox, (255, 0, 255), rt)
        # Top Left  x,y
        cv2.line(img, (x, y), (x + l, y), (255, 0, 255), t)
        cv2.line(img, (x, y), (x, y + l), (255, 0, 255), t)
        # Top Right  x1,y
        cv2.line(img, (x1, y), (x1 - l, y), (255, 0, 255), t)
        cv2.line(img, (x1, y), (x1, y + l), (255, 0, 255), t)
        # Bottom Left  x,y1
        cv2.line(img, (x, y1), (x + l, y1), (255, 0, 255), t)
        cv2.line(img, (x, y1), (x, y1 - l), (255, 0, 255), t)
        # Bottom Right  x1,y1
        cv2.line(img, (x1, y1), (x1 - l, y1), (255, 0, 255), t)
        cv2.line(img, (x1, y1), (x1, y1 - l), (255, 0, 255), t)
        return img

class Servo_device:
    def __init__(self):
        self.butt_angle = 5
        self.servo_position = [100, 100]
        self.set()
        
    def set(self):
        requests.get("http://192.168.137.51:5000/setAngle?s1={}&s2={}".format(self.servo_position[0], self.servo_position[1]))
        
    def buff(self, x_b, y_b):
        x, y = self.servo_position
        if x < 170 or x > 10:
            self.servo_position[0] += x_b
        if y < 170 or y > 10:
            self.servo_position[1] += y_b
        self.set()
            

    def up(self):
        print("上偏移", self.servo_position)
        self.buff(0, -self.butt_angle)
        
    def down(self):
        print("下偏移", self.servo_position)
        self.buff(0, self.butt_angle)
        
    def left(self):
        print("左偏移", self.servo_position)
        self.buff(-self.butt_angle, 0)
        
    def right(self):
        print("右偏移", self.servo_position)
        self.buff(self.butt_angle, 0)

def main():
    cap = cv2.VideoCapture(1)
    pTime = 0
    detector = FaceDetector()
    ser = Servo_device()
    while True:
        success, img = cap.read()
        if not success:
            continue
        img = cv2.flip(img, -1)
        try:
            img, bboxs = detector.findFaces(img)
            print(bboxs)
            x, y, w, h = bboxs[0][1]
            cv2.circle(img, (int(x+w/2), int(y+h/2)), radius=2, color=(255,0,0), thickness=-1)
            if x+w/2 > 220:
                ser.left()
            if x+w/2 < 420:
                ser.right()
            if y+h/2 < 140:
                ser.up()
            if y+h/2 > 340:
                ser.down()
        except BaseException:
            pass
        cTime = time.time()
        fps = 1 / (cTime - pTime)
        pTime = cTime
        cv2.putText(img, f'FPS: {int(fps)}', (20, 70), cv2.FONT_HERSHEY_PLAIN, 3, (0, 255, 0), 2)
        cv2.imshow("人脸追踪", img)
        cv2.waitKey(1)


if __name__ == "__main__":
    main()
